#include "kuruma.h"
#include "clmotor.h"

void test_forward_speed(void) {
	u16 t=0;
	kuruma_enable();
	int now1,now2;
	while(1)
	{
		now1 = clmotor_get_now_rpm(&ml);
		now2 = clmotor_get_now_rpm(&mr);
		printf("%c%c%c%c%c%c",0xff,now1,now1>>8,now2,now2>>8,0xfe);
		//printf("%d %d\n",now1,(int)clmotor_get_now_rpm(&m2));
		//printf("t:%d, m1: %d %.1f %.1f m2: %d %.1f %.1f\n",t, m1.speed.encoder.count, clmotor_get_now_rpm(&m1), m1.pid.target, m2.speed.encoder.count, clmotor_get_now_rpm(&m2), m2.pid.target);
		if(t == 1) {
			clmotor_set_target(&ml,50);
			clmotor_set_target(&mr,-50);
		}
		if(t == 150) {
			clmotor_set_target(&ml,100);
			clmotor_set_target(&mr,-100);
		}
		if(t == 300) {
			clmotor_set_target(&ml,0);
			clmotor_set_target(&mr,0);
		}
		if(t == 400) {
			t = 0;
		}

		delay_ms(20);
		t++;
	}
}
